/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "robotbase.h"

#include "ethercaterror.h"
#include "robotmanager.h"
//#include "robotengine.h"
#include "powermanager.h"
#include <stdio.h>
#include "timetool.h"
#include "messagelog.h"
#include "ethercatVendorId.h"

extern PowerManager* p_powerManger;


RobotBase::RobotBase()
{
    isInitialOk=false;
    logicRobotIo=NULL;
    fpServoDataFile=NULL;
    fpFlush=false;
    servo_data_id=0;
    size_logicalAxisArryCopy=0;
    cycle_us=1000;
    cycle_ns=cycle_us*1000;
    tmpTickCount=0;
    errorCount=1;

}

RobotBase::~RobotBase()
{
    if( NULL!=logicRobotIo )
        delete logicRobotIo;
}

int RobotBase::getCanopenAxisByAlias(int alias, EcatMotor *motorOut)
{
    for(int i=0;i<canopenAxis.size();i++)
    {
        if(canopenAxis[i]->alias==alias)
        {
            motorOut==canopenAxis[i];
            return 1;
        }
    }
    motorOut==NULL;
    return -1;
}

int RobotBase::getMotorVendorId(int axisId, unsigned int &vendorOut)
{
    if(axisId>=0 && axisId<size_logicalAxisArryCopy)
    {
        vendorOut=logicalAxisArryCopy[axisId]->getVendorId();
        return 1;
    }
    else
    {
        return -1;
    }
    return 1;
}

int RobotBase::getMotorErrorCode(int axisId, unsigned short &errorCodeOut)
{
    if(axisId>=0 && axisId<size_logicalAxisArryCopy)
    {
        errorCodeOut=logicalAxisArryCopy[axisId]->errorCode;
        return 1;

    }
    else
    {
        return -1;
    }
    return 1;
}

int RobotBase::getMotorStatusWord(int axisId, unsigned short &statusWordOut)
{
    if(axisId>=0 && axisId<size_logicalAxisArryCopy)
    {
        statusWordOut=logicalAxisArryCopy[axisId]->stateWord;
        return 1;
    }
    else
    {
        return -1;
    }
    return 1;
}

bool RobotBase::getInitialStatus()
{
    return isInitialOk;
}

int RobotBase::getDi(int index,bool &diOut)
{
    diOut=logicRobotIo->getDi(index);
    return 1;
}

int RobotBase::getAi(int index, double &aiOut)
{
    aiOut=logicRobotIo->getAi(index);
    return 1;
}

int RobotBase::getMaganeticDi(QVector<bool> &diOut)
{
    diOut.resize(8);
    for(int i=0;i<diOut.size();i++)
    {
        diOut[i]=logicRobotIo->getDi(i+4);
//        modbusTcpIoModule[]
    }
    return 1;
}

int RobotBase::add_ethercat_node_info(S_EcatDevInfo* nodeInfo)
{
   ethercat_node_info.append(nodeInfo);
   return 1;
}

int RobotBase::getMotorControlMode(int axisIndex,E_MOTOR_CONTROL_MODE& modeOut)
{
    if(axisIndex<0 || axisIndex>=size_logicalAxisArryCopy)
    {
        return -1;
    }
    modeOut=logicalAxisArryCopy[axisIndex]->getControlMode();
    return 1;
}

int RobotBase::setMotorControlMode(int axisIndex,E_MOTOR_CONTROL_MODE modeIn)
{
    if(axisIndex<0 || axisIndex>=size_logicalAxisArryCopy)
    {
        return -1;
    }
    else if(modeIn>E_MOTOR_CONTROL_END ||modeIn<=E_MOTOR_CONTROL_START )
    {
        return -2;
    }
    logicalAxisArryCopy[axisIndex]->setControlMode(modeIn);
    return 1;
}

int RobotBase::initialMotorControlMode()
{
    //逻辑轴
    for(int i=0;i<size_logicalAxisArryCopy;i++)
    {
        logicalAxisArryCopy[i]->setControlMode(robotInfo.motorControlMode[i]);
    }


    for(int i=0;i<size_logicalAxisArryCopy;i++)
    {
        p_powerManger->setMotorControlMode(robotInfo.robotId,i,robotInfo.motorControlMode[i]);
    }


    return 1;
}

int RobotBase::get_ethercat_node_info(int index, S_EcatDevInfo* nodeInfo)
{
//   ethercat_node_info.append(nodeInfo);//bug
   if(index>=0 && index<ethercat_node_info.size())
   {
        nodeInfo=ethercat_node_info[index];
        return 1;
   }
   else
   {
       return -1;
   }
   return 1;
}


int RobotBase::initRobotBase( int robotId, QString folder )
{

//    isSetHomeOffsetOk=true;
    isQuickStopCtl=false;
    isRobotServoOnFlag=false; // HMI伺服控制标志位
    isWarningState=false; // 警告标志
    isFaultOccurred=false;// 报警标志/故障发生
    isMapOk=false; //检测映射有效性标志
    isLinkOk=false;//检测映射有效性标志

    test_acceleration=test_acceleration_set=20;//加速时间设置值/ms
    test_target_increment_tmp=test_target_increment_set=0;//目标增量设置值
    test_axis_set=0;//指定轴(-1=all axis)
    test_dir_tmp=test_dir_set=0;//(0=反向, 1=正向)
    test_run_set=false;
    test_acceleration=0;//加速时间/Sint
    test_dir_ms=0;
    test_count=0;
    get_data_flag=0;

    robotInfo.robotId = robotId;
    robotInfo.robotFolder=folder;
    int returnFlag;
    returnFlag=readRobotInfoXml( D_TCRCONTROLLER_DATA_PATH + folder +"/config/robotInfo.xml" );
    if(1!=returnFlag)
    {
        qDebug()<<"readRobotInfoXml robotInfo.xml error!robotId="<<robotId;
        addMsg(ENUM_MSG_ERROR, "ECAT", "ROBOTBASE", 5603,  robotId,returnFlag  );
        return -1;
    }
    QString tmpIoConfigPath=D_TCRCONTROLLER_DATA_PATH + robotInfo.robotFolder + "/config/"  +  robotInfo.ioMapXML  ;
    logicRobotIo = new RobotIo(tmpIoConfigPath);
    returnFlag=logicRobotIo->readXmlConfig( tmpIoConfigPath );
    if(1!=returnFlag)
    {
        qDebug()<<"readXmlConfig iomap.xml error!robotId="<<robotId;
        addMsg(ENUM_MSG_ERROR, "ECAT", "RobotIo", 5600,  robotId ,returnFlag );
        return -2;
    }


    for(int i=0;i<logicRobotIo->allIoMap.size();i++)
    {
        int alias = logicRobotIo->allIoMap.at(i).nodeId;
        if(E_DEVICE_PROTOCOL_ETHERCAT==logicRobotIo->allIoMap.at(i).deviceProtocol)
        {
            if( !robotInfo.ethercat_ioAlias.contains(alias) )
            {
                robotInfo.ethercat_ioAlias.append(alias);
            }
        }
        else if(E_DEVICE_PROTOCOL_MODBUS_TCP==logicRobotIo->allIoMap.at(i).deviceProtocol)
        {
            if( !robotInfo.modbus_tcp_ioAlias.contains(alias) )
            {
                robotInfo.modbus_tcp_ioAlias.append(alias);
            }
        }
        else if(E_DEVICE_PROTOCOL_MODBUS_RTU==logicRobotIo->allIoMap.at(i).deviceProtocol)
        {
            if( !robotInfo.modbus_rtu_ioAlias.contains(alias) )
            {
                robotInfo.modbus_rtu_ioAlias.append(alias);
            }
        }
        else if(E_DEVICE_PROTOCOL_CANOPEN==logicRobotIo->allIoMap.at(i).deviceProtocol)
        {
            if( !robotInfo.canopen_ioAlias.contains(alias) )
            {
                robotInfo.canopen_ioAlias.append(alias);
            }
        }
    }

    logicAxis.resize(robotInfo.robotMotorCount+robotInfo.extMotorCount);
    for(int i=0;i<logicAxis.size();i++)
    {
        logicAxis[i]=NULL;
    }

    return 1;

}


//==============for api========================

//未标明(-1=所有机器人)的函数，不能传递-1参数
// pthread_mutex_unlock( &ecatThreadMutex );
int RobotBase::createTimeMeasure( )
{
#if D_IF_DEBUG
    int pttime=15000000/cycle_us;//15s 次数打印
    int min_v[3]={-10000,-20000,-50000};
    int max_v[3]={10000,20000,50000};
    timer_id[E_TIME_ROBOT_MATH_P2]=TimeTool::getInstance()->creatTimer( "custom_step2_algorithmFunctioncall", cycle_ns, pttime,min_v, max_v, false );//
#endif
    return 1;
}
int RobotBase::setDcCycle(int cycleTime)
{
    if(0==cycleTime)
    {
        qDebug()<<"error,,,,0==cycle_us";
        addMsg(ENUM_MSG_ERROR, "ECAT", "Robotbase", 5608,  robotInfo.robotId );
        return -1;
    }
    cycle_us=cycleTime;
    cycle_ns=cycle_us*1000;
    return 1;
}

EN_BATTERY_STATUS  RobotBase::isRobotMotorBatteryOk_ecat_motor(int axisId,uint vendorErrorCode)
{

    if( logicalAxisArryCopy[axisId]->errorCode!=0   )
    {
        if(axisId<0 || axisId>size_logicalAxisArryCopy)
        {
            addMsg(ENUM_MSG_ERROR, "ECAT", "Robotbase", 5607,  robotInfo.robotId ,axisId );
            return EN_BATTERY_STATUS_AXIS_NUMBER_ERROR;
        }
        int tmpVendorId=logicalAxisArryCopy[axisId]->getVendorId();
        int axisReturn=EthercatError::isRobotMotorBatteryOk(tmpVendorId
                                    ,logicalAxisArryCopy[axisId]->errorCode,vendorErrorCode);
        if(axisReturn==0)
        {
            return EN_BATTERY_STATUS_LOOSE_VOLTAGE;
        }
        else if(axisReturn==-100)
        {
            return EN_BATTERY_STATUS_UNKNOW_VENDOR;
        }
        else if(axisReturn==1)
        {
            return EN_BATTERY_STATUS_NORMAL;
        }

    }

    return EN_BATTERY_STATUS_NORMAL;
}

bool  RobotBase::isRobotMotorBattoryOk_modbus_rtu_motor()
{
    // todo
    return true;
}

bool  RobotBase::isRobotMotorBattoryOk_modbus_tcp_motor()
{
    // todo
    return true;
}

EN_BATTERY_STATUS RobotBase::isRobotMotorBatteryOk(int axisId,uint vendorErrorCode)
{
    if(false==isAxisNumberCorrect())
    {
       return EN_BATTERY_STATUS_AXIS_SUM_ERROR;
    }

    return  isRobotMotorBatteryOk_ecat_motor(axisId,vendorErrorCode);
//    if(false==isRobotMotorBatteryOk_ecat_motor(vendorErrorCode))
//    {
//        return false;
//    }

//    if(false==isRobotMotorBattoryOk_modbus_rtu_motor())
//    {
//        return false;
//    }

//    if(false==isRobotMotorBattoryOk_modbus_tcp_motor())
//    {
//        return false;
//    }


//    return true;

}

bool RobotBase::setControlWordDebug(unsigned short controlWordIn)
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    isRobotServoOnFlag=true;
    for(int i=0;i<logicAxis.size();i++)
    {
        logicAxis.at(i)->setControlWordDebug( controlWordIn );
    }
    return true;
}

bool RobotBase::setControlWordDebug_axis(unsigned short controlWordIn, int axisId)
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }
    if(axisId>=0 && axisId<logicAxis.size())
    {
        logicAxis.at(axisId)->setControlWordDebug( controlWordIn );

        return true;
    }
    return false;
}

/**
 * @brief servoOnRobot   ///伺服使能(机器人所有轴)
 */
bool RobotBase::servoOnRobot( )
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    isRobotServoOnFlag=true;
    for(int i=0;i<logicAxis.size();i++)
    {
        logicAxis.at(i)->setMotorState( eServoEnable );
    }
    return true;
}

bool RobotBase::servoOnRobotAxis(int axisId)
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }
    if(axisId>=0 && axisId<logicAxis.size())
    {
        logicAxis.at(axisId)->setMotorState( eServoEnable );

        return true;
    }
    return false;

}

/**
 * @brief servoSwitch 伺服物理开关 按下/松开
 * @param flag  　ｏｎ＝ｔｒｕｅ
 */
bool RobotBase::servoSwitch( bool flag )
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    ///锁定
    if(  isRobotServoOnFlag  )
    {
        return false;
    }

    if( flag )
    {// on
        for(int i=0;i<logicAxis.size();i++)
        {
            logicAxis.at(i)->setMotorState( eServoEnable );
        }
    }else{
        for(int i=0;i<logicAxis.size();i++)
        {
            logicAxis.at(i)->setMotorState( eServoOff );
        }
    }

    return true;
}

/**
 * @brief servoOffRobot   ///关闭伺服(机器人所有轴)
 */
bool RobotBase::servoOffRobot( )
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }
    isRobotServoOnFlag=false;
    for(int i=0;i<logicAxis.size();i++)
    {
        logicAxis.at(i)->setMotorState( eServoOff );
    }

    return true;
}

bool RobotBase::servoOffRobotAxis(int axisId)
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    logicAxis.at(axisId)->setMotorState( eServoOff );

    return true;
}

/**
 * @brief quickStopRobot ///伺服立即停止(机器人所有轴)
 */
bool RobotBase::quickStopRobot( )
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    isQuickStopCtl=true;
    isRobotServoOnFlag=false;
    for(int i=0;i<logicAxis.size();i++)
    {
        logicAxis.at(i)->setMotorState( eServoQuickStop );
    }
    return true;
}

/**
 * @brief setHomeOffsetRobot  ///零点设定(机器人所有轴)
 * @param valueList
 */
//int RobotBase::setHomeOffsetRobot( QVector<int>&valueList )
//{
//    if(logicAxis.size()>0 && logicAxis.at(0)->stateWord!=0)
//    {
//        isSetHomeOffsetOk=true;
//        for(int i=0;i<logicAxis.size();i++)
//        {
//            if(valueList.size()>i){
//                logicAxis.at(i)->homeOffset=valueList.at(i);
//                logicAxis.at(i)->targetPosition
//                        = logicAxis.at(i)->encoderValue
//                        = logicAxis.at(i)->actPosition-logicAxis.at(i)->homeOffset;
//            }

//            printf( "机器人 %d，零点标定 ok. %d \n" ,robotInfo.robotId, valueList.at(i) );
//        }
//        tmp_homeoffset.clear();
//        return 1;
//    }else{
//        tmp_homeoffset=valueList;
//        isSetHomeOffsetOk=false;
//        //printf( "机器人 %d，零点标定 failed. \n" ,robotInfo.robotId );
//        return 0;
//    }
//}


/**
 * @brief setControlWordRobot 写控制字(机器人所有轴)
 * @param value 控制命令
 */
bool  RobotBase::setControlWordRobot( int value )
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }
    for(int i=0;i<logicAxis.size();i++)
    {
        logicAxis.at(i)->controlWord=value;
    }
    return true;
}

/**
 * @brief getServoDataRobot  ///获取轴的各种数据(机器人所有轴)
 * @return
 */
QVector<ServoState> RobotBase::getServoDataRobot( )
{
    QVector<ServoState> sdata;
    if(false==isAxisNumberCorrect())
    {
       return sdata;
    }


    sdata.resize( logicAxis.size() );
    for(int i=0;i<logicAxis.size();i++)
    {
        LogicMotor* ax = logicAxis.at(i);

        sdata[i].isServoOn= ax->isServoOn();
        sdata[i].isServoQuickStop= ax->isServoQuickStop();
        sdata[i].isServoPowerOn= ax->isServoPowerOn();
        sdata[i].isServoTargetReached= ax->isServoTargetReached();
        //            sdata[i].isServoFllowingError= ax->isServoFllowingError();
        //            sdata[i].isServoHoming= ax->isServoHoming();
        //            sdata[i].isServoHomingOk= ax->isServoHomingOk();
        //            sdata[i].isServoHomingErr= ax->isServoHomingErr();

        sdata[i].position= ax-> encoderValue;//getMotorPositionNode();
        sdata[i].speed= ax-> actVelocity;
        sdata[i].torque= ax-> actTorque;
        sdata[i].power= 50;
        sdata[i].fllowingOffset = ax-> actFollowErr; //实际跟随误差(计算值)

        if( isFaultOccurred )
        {//error
            sdata[i].errorCode=  ax->errorCode;
            sdata[i].errorString="errorString";
        }
        else if( isWarningState )
        {//warm
            sdata[i].warnCode=  ax->errorCode;
            sdata[i].warnString="warnString";
        }
    }
    return sdata;
}

/**
 * @brief resetErrorRobot //报警复位(机器人所有轴)
 */
bool RobotBase::resetErrorRobot()
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    for(int i =0 ;i< logicAxis.size();i++)
    {
        if(false==logicAxis.at(i)->isServoOn())
        {
           logicAxis.at(i)->setMotorState(eServoFaultReset);
        }

    }
    isFaultOccurred=false;
    isWarningState=false;
    return true;
}

/**
 * @brief robotAxisToEthercatNode 获取指定轴的站号
 * @return 1 if ok
 */
int RobotBase::robotAxisToEthercatNode( int axis, int &returnNode ,
                                        E_DEVICE_PROTOCOL &protocolOut)
{
    if(axis<size_logicalAxisArryCopy && 0<=axis)
    {
        if(E_DEVICE_PROTOCOL_ETHERCAT==logicalAxisArryCopy[axis]->getProtocol())
        {
            //ethercat
            if(axis>=ethercat_node_info.size())
            {
                return 0;
            }
            returnNode = this->ethercat_node_info.at(axis)->devInfo.node;
            protocolOut=E_DEVICE_PROTOCOL_ETHERCAT;
//            qDebug()<<"robotAxisToEthercatNode"<<returnNode<<"alias"<<this->ethercat_node_info.at(axis)->devInfo.alias;
            return 1;
        }
        else if(E_DEVICE_PROTOCOL_CANOPEN==logicalAxisArryCopy[axis]->getProtocol())
        {
            //canopen
            returnNode = logicalAxisArryCopy[axis]->getAlias();
            protocolOut=E_DEVICE_PROTOCOL_CANOPEN;
            return 1;
        }
    }
    return 0;
}

int RobotBase::deviceAliasToEthercatNode(int deviceAliasIn, int &returnNode, E_DEVICE_PROTOCOL &protocolOut)
{

        if(0<ethercat_node_info.size())
        {
            //ethercat
            for(int i=0;i<ethercat_node_info.size();i++)
            {
                if(deviceAliasIn==ethercat_node_info.at(i)->devInfo.alias)
                {
                    returnNode =ethercat_node_info.at(i)->devInfo.node;
                    protocolOut=E_DEVICE_PROTOCOL_ETHERCAT;
                    return 1;
                }
            }
            return 0;

        }
        else
        {
            //canopen
            returnNode = deviceAliasIn;
            protocolOut=E_DEVICE_PROTOCOL_CANOPEN;
            return 1;
        }

    return 0;
}

/**
 * @brief isServoOnRobot   获取机器人伺服ON状态 （所有轴ON才正确）,除去独立使能控制轴
 */
bool RobotBase::isServoOnRobot()
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    for(int i=0; i<logicAxis.size(); i++ )
    {
        if(  !logicAxis.at(i)->isServoOn() )
        {
            return false;
        }
    }
    return true;
}

bool RobotBase::isServoOnRobotAxis(int axisId)
{
    if(axisId<0 || axisId >=logicAxis.size())
    {
       return false;
    }

    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    if(  !logicAxis.at(axisId)->isServoOn() )
    {
        return false;
    }
    else
    {
        return true;
    }

    return true;
}

int RobotBase::getServoOnStatus(QVector<bool> &flagReturn)
{
    if(false==isAxisNumberCorrect())
    {
       return -1;
    }

    flagReturn.clear();

    for(int i=0; i<logicAxis.size(); i++ )
    {
        flagReturn.append(logicAxis.at(i)->isServoOn());
    }
    return 1;
}

/**
 * @brief setMotorPositionRobot 写编码器的值
 * @param vPos 写值
 */
bool  RobotBase::setMotorPositionRobot(QVector<int> vPos)
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    for(int i=0; i<logicAxis.size(); i++ )
    {
        if(i<vPos.size())
            logicAxis.at(i)->targetPosition = vPos.at(i);
    }
    return true;
}

/**
 * @brief setMotorPositionAxis 写编码器的值
 * @param axis
 * @param pos 写值
 */
bool RobotBase::setMotorPositionAxis(int axis, int pos)
{
    if(false==isAxisNumberCorrect())
    {
       return false;
    }

    if( axis < this->logicAxis.size() )
    {
        this->logicAxis.at(axis)->targetPosition = pos;
    }
    return true;
}

/**
 * @brief getMotorPositionRobot  计算后的编码（带零点偏移）
 * @return 编码器的值
 */
QVector<int> RobotBase::getMotorPositionRobot()
{
    QVector<int> pos;
    if(false==isAxisNumberCorrect())
    {
       return pos;
    }


    pos.resize( logicAxis.size() );
    for(int i=0; i<logicAxis.size(); i++ )
    {
        pos[i]=logicAxis.at(i)->encoderValue;
    }
    return pos;
}

/**
 * @brief getMotorPositionRobot_real  读上来的真实编码器值 （真实编码器值）
 * @return 编码器的值
 */
QVector<int> RobotBase::getMotorPositionRobot_real( )
{
    QVector<int> pos;
    if(false==isAxisNumberCorrect())
    {
       return pos;
    }

    pos.resize( logicAxis.size() );
    for(int i=0; i<logicAxis.size(); i++ )
    {
        pos[i]=logicAxis.at(i)->actPosition;
    }
    return pos;
}

void RobotBase::addMsg(int messageLevel,
                       std::string componentName,
                       std::string messageType,
                       int messageCode,
                       int _robotId,
                       int parameter1 ,
                       int parameter2 ,
                       int parameter3 ,
                       int parameter4  )
{

    struct timeval tv_begin ;
    gettimeofday(&tv_begin, NULL);

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotInfo.robotId;
    tmpMsg.messageCode = messageCode;
    tmpMsg.time = tv_begin.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {
    case 5610:
    {
        infomationStr = QObject::tr( "获取电机编码器线数错误！"
                                     "获得电机数=%1,正确电机数=%2")
                .arg( parameter1 )
                .arg( parameter2 );

    }
        break;
    case 5609:
    {
        infomationStr = QObject::tr( "电机(%1号)发生告警，请检查是否需要更换电池！检查0x2224对象字典是否等于0xff39").arg( parameter1+1 );//0xff39

    }
        break;
    case 5608:
    {
        infomationStr = QObject::tr( "error ,cycle_us=0");

    }
        break;
    case 5607:
    {
        infomationStr = QObject::tr( "判断电池状态时出现轴数量信息错误，i=%1").arg(parameter1);

    }
        break;
    case 5606:
    {
        infomationStr = QObject::tr( "算法指针为空");

    }
        break;
    case 5605:
    {
        infomationStr = QObject::tr( "robotInfo.xml轴数量与busDev.xml轴数量不一致错误！"
                                     "logicalAxis=%1,ethercatAxis=%2,modbusAxis=%3,canopenAxis=%4")
                .arg( parameter1 )
                .arg( parameter2 )
                .arg( parameter3 )
                .arg( parameter4 );

    }
        break;
    case 5604:
    {
        infomationStr = QObject::tr( "modus_rtu设备运行错误: error return = %1, busId=%2 ")
                .arg( parameter1 )
                .arg( parameter2 );
    }
        break;
    case 5603:
    {
        infomationStr = QObject::tr( "robotInfo.xml配置错误: error return = %1")
                .arg( parameter1 ) ;
    }
        break;
    case 5600:
    {
        infomationStr = QObject::tr( "iomap.xml配置错误:  error return= %1 ")
                .arg( parameter1 ) ;
    }
        break;
    case 5601:
    {
        infomationStr = QObject::tr( "IO映射配置错误:  设备ID: %1, 无此IO类型: %2,  或超出设备索引: %3  ")
                .arg( parameter1 )
                .arg( parameter2 )
                .arg( parameter3 ) ;
    }
        break;
    case 5602:
    {
        infomationStr = QObject::tr( "IO映射配置错误:   无此物理设备ID : %1  ")
                .arg( parameter1 );
    }
        break;
    case 5020:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(_robotId);
        infomationStr = infomationStr + QObject::tr(",轴")
                + QString::number(parameter1+1)
                +  " ," +QObject::tr("回零步骤操作超时 1 !");
    }
        break;
//    case 5034:
//    {
//        QString tempError;
//        QString errorInfo=EthercatError::geterror( parameter3, parameter2 );
//        tempError.clear();
//        tempError.sprintf("%02x",parameter2);
//        infomationStr =  QObject::tr("伺服报警:机器人:")  + QString::number(_robotId)
//                + QObject::tr(", 轴:") + QString::number(parameter1+1)
//                + QObject::tr(", 错误码:0x")  + tempError+" ,";
//        infomationStr+=errorInfo;
//    }
//        break;
    case 5040:
    {
        infomationStr =  QObject::tr("零点标定数据更新失败，ethercat未完成初始化！")
                + QString::number(_robotId);
    }
        break;
    case 5041:
    {
        QString tempError;
        tempError.sprintf("%02x",parameter2);
        infomationStr =  QObject::tr("机器人:")  + QString::number(_robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," + QObject::tr("伺服发生警告,警告码 :0x")+ tempError
                + EthercatError::getwarn( parameter3, parameter2 );
    }
        break;
    case 142:
    {
        infomationStr =  QObject::tr("机器人:")
                + QString::number(_robotId)
                + QObject::tr(" ,未完成零点标定，无法使能 :");
    }
        break;
    }

    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);
}

//==end


//更新机器人报警
int RobotBase::checkError( )
{
    checkEthercatDeviceError( );
    checkModbusRtuDeviceError( );
    checkModbusTcpDeviceError( );
    checkCanopenDeviceError();
    return 1;
}

int RobotBase::checkModbusRtuDeviceError( )
{
    //todo
    for(int i=0; i<modbusRtuAxis.size(); i++ )
    {

        if( modbusRtuAxis.at(i)->isErrorOccured() )
        {//error

                isFaultOccurred= true;

        }

    }
    // if( isFaultOccurred || customErrorSearch() )
    if( isFaultOccurred )
    {
        // 错误发生
        if(!isQuickStopCtl)
            quickStopRobot();
    }else{
        isQuickStopCtl=false;
    }
    if( isFaultOccurred )
    {
        return -1;
    }

    return 1;
}

int RobotBase::checkModbusTcpDeviceError( )
{
    //todo
    for(int i=0; i<modbusTcpAxis.size(); i++ )
    {

        if( modbusTcpAxis.at(i)->isErrorOccured() )
        {//error

                isFaultOccurred= true;

        }

    }
    // if( isFaultOccurred || customErrorSearch() )
    if( isFaultOccurred )
    {
        // 错误发生
        if(!isQuickStopCtl)
            quickStopRobot();
    }else{
        isQuickStopCtl=false;
    }
    if( isFaultOccurred )
    {
        return -1;
    }

    return 1;
}


int RobotBase::checkEthercatDeviceError( )
{
    isWarningState = false;
    isFaultOccurred = false;
    for(int i=0; i<ethercatAxis.size(); i++ )
    {
        switch( ethercat_node_info.at(i)->devInfo.devType )
        {
        case E_EcatDev_Servo:
        {
            unsigned int errorCodeOut,vendorIdOut;
            int aliasOut;
            ethercatAxis.at(i)->checkEthercatDeviceError(isWarningState,isFaultOccurred,errorCodeOut,vendorIdOut,aliasOut);

            break;//  motor
        }
        case E_EcatDev_Coupler:
            break;//
        case E_EcatDev_IoModule:
            break;
        case E_EcatDev_TypeErr:
        default:
            break;
        }
    }
    // if( isFaultOccurred || customErrorSearch() )
    if( isFaultOccurred )
    {
        // 错误发生
        if(!isQuickStopCtl)
            quickStopRobot();
    }else{
        isQuickStopCtl=false;
    }
    if( isFaultOccurred )
    {
        return -1;
    }

    return 1;
}



int RobotBase::checkCanopenDeviceError( )
{
    isWarningState = false;
    isFaultOccurred = false;
    for(int i=0; i<canopenAxis.size(); i++ )
    {
        unsigned int errorCodeOut,vendorIdOut;
        int aliasOut;
        canopenAxis.at(i)->checkCanopenDeviceError(isWarningState,isFaultOccurred,errorCodeOut,vendorIdOut,aliasOut);

    }


    // if( isFaultOccurred || customErrorSearch() )
    if( isFaultOccurred )
    {
        // 错误发生
        if(!isQuickStopCtl)
            quickStopRobot();
    }
    else
    {
        isQuickStopCtl=false;
    }
    if( isFaultOccurred )
    {
        return -1;
    }

    return 1;
}


//更新机器io
bool RobotBase::updateEthercatIoData( )
{
    if( !checkIoMapStatus() )
    {
        isMapOk=false;
        return false;
    }

    if(NULL==logicRobotIo)
    {
        return false;
    }


    //DO
    for(int i=0; i<logicRobotIo->ioDoMap.size(); i++)
    {
        EcatIo* pIodata= ecatIoDeviceDataMap.value( logicRobotIo->ioDoMap.at(i)->nodeId, NULL );
        if(pIodata!=NULL){
            pIodata->setDo(  logicRobotIo->ioDoMap.at(i)->deviceValueIndex, logicRobotIo->ioDo[ logicRobotIo->ioDoMap.at(i)->logicValueIndex ] );
        }
    }
    //SO
    for(int i=0; i<logicRobotIo->ioSoMap.size(); i++){
        EcatIo* pIodata= ecatIoDeviceDataMap.value( logicRobotIo->ioSoMap.at(i)->nodeId, NULL );
        if(pIodata!=NULL){
            pIodata->setDo(  logicRobotIo->ioSoMap.at(i)->deviceValueIndex, logicRobotIo->ioSo[ logicRobotIo->ioSoMap.at(i)->logicValueIndex ] );
        }
    }
    //AO
    for(int i=0; i<logicRobotIo->ioAoMap.size(); i++){
        EcatIo* pIodata= ecatIoDeviceDataMap.value( logicRobotIo->ioAoMap.at(i)->nodeId, NULL );
        if(pIodata!=NULL){
            pIodata->setAo( logicRobotIo->ioAoMap.at(i)->deviceValueIndex, logicRobotIo->ioAo[ logicRobotIo->ioAoMap.at(i)->logicValueIndex ] );

        }
    }

    //DI
    for(int i=0; i<logicRobotIo->ioDiMap.size(); i++){
        EcatIo* pIodata= ecatIoDeviceDataMap.value( logicRobotIo->ioDiMap.at(i)->nodeId, NULL );
        if(pIodata!=NULL){
            logicRobotIo->ioDi[ logicRobotIo->ioDiMap.at(i)->logicValueIndex ] =  pIodata->getDi( logicRobotIo->ioDiMap.at(i)->deviceValueIndex )  ;
        }
    }
    //SI
    for(int i=0; i<logicRobotIo->ioSiMap.size(); i++){
        EcatIo* pIodata= ecatIoDeviceDataMap.value( logicRobotIo->ioSiMap.at(i)->nodeId, NULL );
        if(pIodata!=NULL){
            logicRobotIo->ioSi[ logicRobotIo->ioSiMap.at(i)->logicValueIndex ] = pIodata->getDi( logicRobotIo->ioSiMap.at(i)->deviceValueIndex );
        }
    }
    //AI
    for(int i=0; i<logicRobotIo->ioAiMap.size(); i++){
        EcatIo* pIodata= ecatIoDeviceDataMap.value( logicRobotIo->ioAiMap.at(i)->nodeId, NULL );
        if(pIodata!=NULL){
            logicRobotIo->ioAi[ logicRobotIo->ioAiMap.at(i)->logicValueIndex ] = pIodata->getAi( logicRobotIo->ioAiMap.at(i)->deviceValueIndex );
        }
    }
    //encoder 无映射



    testForDiTrigCount();
    return true;



}

void RobotBase::testForDiTrigCount()
{
    int testIndex=4;
    static int trigCount=0;
    static bool diOld=false;
    if(false==logicRobotIo->ioDi[ testIndex ] && true==diOld)
    {
        trigCount++;
        qDebug()<<"testForDiTrigCount"<<trigCount;
    }
    diOld=logicRobotIo->ioDi[ testIndex ];

}

bool RobotBase::updateCanIoData()
{
    if( !checkIoMapStatus() )
    {
        isMapOk=false;
        return false;
    }

    if(NULL==logicRobotIo)
    {
        return false;
    }

        //DO
        for(int i=0; i<logicRobotIo->ioDoMap.size(); i++)
        {
            EcatIo* pIodata= canioDeviceDataMap.value( logicRobotIo->ioDoMap.at(i)->nodeId, NULL );
            if(pIodata!=NULL)
            {
                pIodata->can_doMemory[logicRobotIo->ioDoMap.at(i)->deviceValueIndex]=
                        logicRobotIo->ioDo[ logicRobotIo->ioDoMap.at(i)->logicValueIndex ];
//                pIodata->setDo(  logicRobotIo->ioDoMap.at(i)->deviceValueIndex, logicRobotIo->ioDo[ logicRobotIo->ioDoMap.at(i)->logicValueIndex ] );
            }
        }
        //SO
        for(int i=0; i<logicRobotIo->ioSoMap.size(); i++){
            EcatIo* pIodata= canioDeviceDataMap.value( logicRobotIo->ioSoMap.at(i)->nodeId, NULL );
            if(pIodata!=NULL)
            {
                pIodata->can_doMemory[logicRobotIo->ioSoMap.at(i)->deviceValueIndex]=
                        logicRobotIo->ioSo[ logicRobotIo->ioSoMap.at(i)->logicValueIndex ];
//                pIodata->setDo(  logicRobotIo->ioSoMap.at(i)->deviceValueIndex, logicRobotIo->ioSo[ logicRobotIo->ioSoMap.at(i)->logicValueIndex ] );
            }
        }
        //AO
        for(int i=0; i<logicRobotIo->ioAoMap.size(); i++){
            EcatIo* pIodata= canioDeviceDataMap.value( logicRobotIo->ioAoMap.at(i)->nodeId, NULL );
            if(pIodata!=NULL)
            {
//                pIodata->setAo( logicRobotIo->ioAoMap.at(i)->deviceValueIndex,
//                                logicRobotIo->ioAo[ logicRobotIo->ioAoMap.at(i)->logicValueIndex ] );
                pIodata->can_aoMemory[logicRobotIo->ioAoMap.at(i)->deviceValueIndex]=
                        logicRobotIo->ioAo[ logicRobotIo->ioAoMap.at(i)->logicValueIndex ];

            }
        }

        //DI
        for(int i=0; i<logicRobotIo->ioDiMap.size(); i++){
            EcatIo* pIodata= canioDeviceDataMap.value( logicRobotIo->ioDiMap.at(i)->nodeId, NULL );
            if(pIodata!=NULL)
            {
                logicRobotIo->ioDi[ logicRobotIo->ioDiMap.at(i)->logicValueIndex ]=
                        pIodata->can_diMemory[logicRobotIo->ioDiMap.at(i)->deviceValueIndex];
//                logicRobotIo->ioDi[ logicRobotIo->ioDiMap.at(i)->logicValueIndex ] =  pIodata->getDi( logicRobotIo->ioDiMap.at(i)->deviceValueIndex )  ;
            }
        }
        //SI
        for(int i=0; i<logicRobotIo->ioSiMap.size(); i++){
            EcatIo* pIodata= canioDeviceDataMap.value( logicRobotIo->ioSiMap.at(i)->nodeId, NULL );
            if(pIodata!=NULL)
            {
                logicRobotIo->ioSi[ logicRobotIo->ioSiMap.at(i)->logicValueIndex ]=
                        pIodata->can_diMemory[logicRobotIo->ioSiMap.at(i)->deviceValueIndex];
//                logicRobotIo->ioSi[ logicRobotIo->ioSiMap.at(i)->logicValueIndex ] = pIodata->getDi( logicRobotIo->ioSiMap.at(i)->deviceValueIndex );
            }
        }
        //AI
        for(int i=0; i<logicRobotIo->ioAiMap.size(); i++){
            EcatIo* pIodata= canioDeviceDataMap.value( logicRobotIo->ioAiMap.at(i)->nodeId, NULL );
            if(pIodata!=NULL)
            {
                logicRobotIo->ioAi[ logicRobotIo->ioAiMap.at(i)->logicValueIndex ] =
                        pIodata->can_aiMemory[logicRobotIo->ioAiMap.at(i)->deviceValueIndex];

            }
        }



    return true;

}

int RobotBase::updateInputModbusRtuIodata()
{

    //read di
    for(int i=0;i<logicRobotIo->ioDiMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_RTU==logicRobotIo->ioDiMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusRtuIoModule.size();j++)
            {
                if(modbusRtuIoModule[j]->alias==logicRobotIo->ioDiMap[i]->nodeId)
                {
                    logicRobotIo->ioDi[logicRobotIo->ioDiMap[i]->logicValueIndex]=
                            modbusRtuIoModule[j]->modbus_diMemory[logicRobotIo->ioDiMap[i]->deviceValueIndex];
                    break;
                }
            }

        }


    }

    //read si
    for(int i=0;i<logicRobotIo->ioSiMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_RTU==logicRobotIo->ioSiMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusRtuIoModule.size();j++)
            {
                if(modbusRtuIoModule[j]->alias==logicRobotIo->ioSiMap[i]->nodeId)
                {
                    logicRobotIo->ioSi[logicRobotIo->ioSiMap[i]->logicValueIndex]=
                            modbusRtuIoModule[j]->modbus_diMemory[logicRobotIo->ioSiMap[i]->deviceValueIndex];
                    break;
                }
            }
        }


    }
    return 1;
}

int RobotBase::updateInputModbusTcpIodata()
{

    //read di
    for(int i=0;i<logicRobotIo->ioDiMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_TCP==logicRobotIo->ioDiMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusTcpIoModule.size();j++)
            {
                if(modbusTcpIoModule[j]->alias==logicRobotIo->ioDiMap[i]->nodeId)
                {
                    logicRobotIo->ioDi[logicRobotIo->ioDiMap[i]->logicValueIndex]=
                            modbusTcpIoModule[j]->modbus_diMemory[logicRobotIo->ioDiMap[i]->deviceValueIndex];
//                    qDebug()<<"logicRobotIo->ioDiMap[i]->logicValueIndex"<<logicRobotIo->ioDiMap[i]->logicValueIndex;
//                    qDebug()<<"logicRobotIo->ioDi[logicRobotIo->ioDiMap[i]->logicValueIndex]"
//                            <<logicRobotIo->ioDi[logicRobotIo->ioDiMap[i]->logicValueIndex];
//                    qDebug()<<"logicRobotIo->ioDiMap[i]->deviceValueIndex"<<logicRobotIo->ioDiMap[i]->deviceValueIndex;

                    break;
                }
            }

        }


    }

    //read si
    for(int i=0;i<logicRobotIo->ioSiMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_TCP==logicRobotIo->ioSiMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusTcpIoModule.size();j++)
            {
                if(modbusTcpIoModule[j]->alias==logicRobotIo->ioSiMap[i]->nodeId)
                {
                    logicRobotIo->ioSi[logicRobotIo->ioSiMap[i]->logicValueIndex]=
                            modbusTcpIoModule[j]->modbus_diMemory[logicRobotIo->ioSiMap[i]->deviceValueIndex];
                    break;
                }
            }
        }


    }
    return 1;
}


int RobotBase::updateOutputModbusRtuIodata()
{

    //write do
    for(int i=0;i<logicRobotIo->ioDoMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_RTU==logicRobotIo->ioDoMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusRtuIoModule.size();j++)
            {
                if(modbusRtuIoModule[j]->alias==logicRobotIo->ioDoMap[i]->nodeId)
                {
                    modbusRtuIoModule[j]->modbus_doMemory[logicRobotIo->ioDoMap[i]->deviceValueIndex]=
                             logicRobotIo->ioDo[logicRobotIo->ioDoMap[i]->logicValueIndex];
                    break;
                }
            }

        }


    }

    //write so
    for(int i=0;i<logicRobotIo->ioSoMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_RTU==logicRobotIo->ioSoMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusRtuIoModule.size();j++)
            {
                if(modbusRtuIoModule[j]->alias==logicRobotIo->ioSoMap[i]->nodeId)
                {
                    modbusRtuIoModule[j]->modbus_doMemory[logicRobotIo->ioSoMap[i]->deviceValueIndex]=
                             logicRobotIo->ioSo[logicRobotIo->ioSoMap[i]->logicValueIndex];

//                    qDebug()<<"doindex "<<logicRobotIo->ioSoMap[i]->deviceValueIndex<<" logicIndex"
//                           <<logicRobotIo->ioSoMap[i]->logicValueIndex<<" ="<<
//                             logicRobotIo->ioSo[logicRobotIo->ioSoMap[i]->logicValueIndex];
                    break;
                }
            }

        }


    }

    return 1;

}


int RobotBase::updateOutputModbusTcpIodata()
{

    //write do
    for(int i=0;i<logicRobotIo->ioDoMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_TCP==logicRobotIo->ioDoMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusTcpIoModule.size();j++)
            {
                if(modbusTcpIoModule[j]->alias==logicRobotIo->ioDoMap[i]->nodeId)
                {
                    modbusTcpIoModule[j]->modbus_doMemory[logicRobotIo->ioDoMap[i]->deviceValueIndex]=
                             logicRobotIo->ioDo[logicRobotIo->ioDoMap[i]->logicValueIndex];
                    break;
                }
            }

        }


    }

    //write so
    for(int i=0;i<logicRobotIo->ioSoMap.size();i++)
    {

        if(E_DEVICE_PROTOCOL_MODBUS_TCP==logicRobotIo->ioSoMap[i]->deviceProtocol)
        {

            for(int j=0;j<modbusTcpIoModule.size();j++)
            {
                if(modbusTcpIoModule[j]->alias==logicRobotIo->ioSoMap[i]->nodeId)
                {
                    modbusTcpIoModule[j]->modbus_doMemory[logicRobotIo->ioSoMap[i]->deviceValueIndex]=
                             logicRobotIo->ioSo[logicRobotIo->ioSoMap[i]->logicValueIndex];
                    break;
                }
            }

        }


    }

    return 1;

}


bool RobotBase::setServoData( )
{
    if( p_powerManger!=NULL )
    {
        TimeTool::getInstance()->recordInTime(timer_id[E_TIME_ROBOT_MATH_P2]);
        int mutexFlag;
        QVector<MotorData> mdata;
        //反馈数据填充
        QVector<MotorData> feedbackData;
         QVector<bool> servoOnStatus;
        feedbackData.resize(logicAxis.size());
        servoOnStatus.resize(logicAxis.size());
        for(int  i=0;i< logicAxis.size(); i++)
        {
            if(NULL!=logicAxis.at(i))
            {
                feedbackData[i].position=logicAxis.at(i)->actPosition;
                feedbackData[i].speed=logicAxis.at(i)->actVelocity;
                feedbackData[i].torque=logicAxis.at(i)->actTorque;
                servoOnStatus[i]=logicAxis.at(i)->isServoOn();
            }

         }
        p_powerManger->algorithmFunctioncall(robotInfo.robotId,servoOnStatus,feedbackData,mdata,mutexFlag);
        TimeTool::getInstance()->recordOutTime(timer_id[E_TIME_ROBOT_MATH_P2]);
        //算法线程没有占用trajMutex，则更新电机数据
        if(0==mutexFlag)
        {
            //如果serv off，不要下发数据。hualei (纯粹仿真的时候，会抖动）
            if( mdata.size() && logicAxis.size()==mdata.size() )
            {
                // rt_printf("发送点位数据= " );
//                if(isServoOnRobot()==true)
//                {
//                }
                  for(int  i=0;i< mdata.size(); i++)
                    {
                          if(NULL!=logicAxis.at(i))
                          {
//                              if(3==i)
//                              {
////                                  qDebug()<<"qt vel="<<mdata[i].speed;
//                                  printf("igh motor %d \r\n", mdata[i].speed );
//                              }
                               // rt_printf(" %d , ", mdata[i].position );
                                logicAxis.at(i)->targetPosition=mdata.at(i).position;
                                logicAxis.at(i)->targetTorque=mdata.at(i).torque;
                                logicAxis.at(i)->targetVelocity=mdata.at(i).speed;
//                                logicAxis.at(i)->pulsePerRound=mdata.at(i).pulsePerRound;
//                                logicAxis.at(i)->torqueOffset=mdata.at(i).torqueOffset;// 0x60B2 - 力距漂移
                                logicAxis.at(i)->velocityOffset=mdata.at(i).velocityOffset;// 0x60B1 - 速度漂移     //911MotorTest
//                                logicAxis.at(i)->torqueOffset=0;// 0x60B2 - 力距漂移
//                                logicAxis.at(i)->velocityOffset=0;// 0x60B1 - 速度漂移
                            }
                      }

                get_data_flag=1;
            }
            else
            {
//                printf("motor data size error \r\n" );
//                qDebug()<<"mdata.size()"<<mdata.size()<<"logicAxis.size" <<logicAxis.size() ;
                get_data_flag=0;
            }
         }

    }
    return true;
}

int RobotBase::initialMotorEncoderValue()
{
    do
    {
        sleep(1);
        qDebug()<<"wait for p_powerManger";
    }while(NULL==p_powerManger);

    if( p_powerManger!=NULL )
    {
        QVector<double> pulseOut;
        int waitCount=10;
        int tmpKey;
        do
        {
            sleep(1);
            tmpKey=p_powerManger->getMotorEncoderPulsePerRound(robotInfo.robotId,pulseOut);
            waitCount--;
        }
        while(waitCount>0 && -1==tmpKey);


        if(pulseOut.size()!=size_logicalAxisArryCopy)
        {
            addMsg(ENUM_MSG_ERROR, "RobotBase", "RobotBase", 5610,robotInfo.robotId,
                                                pulseOut.size(),size_logicalAxisArryCopy);
            return -1;
        }


        for(int  i=0;i< size_logicalAxisArryCopy; i++)
        {
               logicalAxisArryCopy[i]->pulsePerRound=pulseOut[i];
        }
    }
    return 1;
}

int RobotBase::initialServoParameter()
{
    while(NULL==p_powerManger)
    {
        usleep(100);
//        qDebug()<<"warnning,,,,NULL==p_powerManger,when initialServoParameter";
    }
    return p_powerManger->initialServoParameter(robotInfo.robotId);

}

//===========end api================



int RobotBase::readRobotInfoXml( QString xmlfile )
{
    bool ok=openXml( xmlfile, QFile::ReadOnly | QFile::Text );
    if(!ok)
    {
        qDebug()<< "ERROR: openXml :"  << xmlfile;
        return -1;
    }

    QDomNode  robNode =  m_root ;

    readXml( findSubNode( robNode, "robotName", ok ), robotInfo.robotName );
    if(!ok)
    {
        return -2;
    }
    readXml( findSubNode( robNode, "robotSetting", ok ), robotInfo.robotSetting );
    if(!ok)
    {
        return -3;
    }
    readXml( findSubNode( robNode, "ioMapXML", ok ), robotInfo.ioMapXML );
    if(!ok)
    {
        return -4;
    }

    readXml( findSubNode( robNode, "robotMotorCount", ok ), robotInfo.robotMotorCount );
    if(!ok)
    {
        return -5;
    }
    readXml( findSubNode( robNode, "extMotorCount", ok ), robotInfo.extMotorCount );
    if(!ok)
    {
        return -6;
    }

    QString pic;
    QString robotSpecification;
    readXml( findSubNode( robNode, "robotSpec", ok ), robotSpecification );
    if(!ok)
    {
        return -7;
    }


    readXml( findSubNode( robNode, "robotType", ok ),  robotInfo.robotType  );
    if(!ok)
    {
        return -8;
    }
    readXml( findSubNode( robNode, "robotPicture", ok ), pic );
    if(!ok)
    {
        return -9;
    }

    robotInfo.robotSpecification = robotSpecification.toStdString();	//机器人规格
    robotInfo.robotFileAddress = QString(D_TCRCONTROLLER_DATA_PATH + robotInfo.robotFolder + "/").toStdString();	//机器人所在的文件路径
    robotInfo.robotPictureAddress =  robotInfo.robotFileAddress + pic.toStdString();//机器人所在的图片路径


    int axisCount= robotInfo.robotMotorCount + robotInfo.extMotorCount;
    robotInfo.motorAlias.resize( axisCount );
    robotInfo.motorDeviceProtocol.resize( axisCount );
    robotInfo.motorControlMode.resize( axisCount );
    robotInfo.motorChannel.resize( axisCount );
    for(int j=0;j< axisCount; j++)
    {
        if(j<robotInfo.robotMotorCount)
        {
            QDomNode devND=findSubNode( robNode, QString("robotNodeAlias%1").arg(j+1), ok );
            if(!ok)
            {
                return -10000-j;
            }
            readXml( devND ,robotInfo.motorAlias[j]  );
            if(false==devND.toElement().hasAttribute("deviceProtocol" ))
            {
                return -10;
            }
            robotInfo.motorDeviceProtocol[j] = (E_DEVICE_PROTOCOL)devND.toElement().attribute("deviceProtocol" ).toUInt();
            if(false==devND.toElement().hasAttribute("controlMode" ))
            {
                return -11;
            }
            robotInfo.motorControlMode[j] = (E_MOTOR_CONTROL_MODE)devND.toElement().attribute("controlMode" ).toUInt();

            if(false==devND.toElement().hasAttribute("motorChannel" ))
            {
                qDebug()<<"error,no find node motorChannel!";
                return -11;
            }
            robotInfo.motorChannel[j] = (E_MOTOR_CONTROL_MODE)devND.toElement().attribute("motorChannel" ).toUInt();

        }
        else
        {
            QDomNode devND=findSubNode( robNode, QString("extNodeAlias%1").arg(j-robotInfo.robotMotorCount+1), ok );
            if(!ok)
            {
                return -10000-j;
            }
            readXml( devND ,robotInfo.motorAlias[j]  );
            if(false==devND.toElement().hasAttribute("deviceProtocol" ))
            {
                return -10;
            }
            robotInfo.motorDeviceProtocol[j] = (E_DEVICE_PROTOCOL)devND.toElement().attribute("deviceProtocol" ).toUInt();


        }
        if(!ok){
            qDebug()<<"ERROR: 获取机器人信息失败2"<<__FILE__<< __LINE__  << j ;
        }
    }

    closeXml();

    return 1;

}


///检查映射有效性
int  RobotBase::checkIoMapStatus()
{
    if( isMapOk )
    {
        return 1;
    }
    isMapOk=true;

    if(NULL==logicRobotIo)
    {
        return -1;
    }

    bool tmpReturn;
    tmpReturn=isIoMapOk();
    if( !tmpReturn )
    {
        isMapOk=false;
        return -2;
    }


    return 1;
}

bool RobotBase::isIoMapOk()
{
    int errType=0;//0=ok, -1=无此类型 或
    bool find=false;
    for( int i=0;i<logicRobotIo->allIoMap.size();i++ )
    {
        //ETHERCAT
        for( int j=0;j<ethercatIoModule.size();j++ )
        {
            if( logicRobotIo->allIoMap.at(i).nodeId==ethercatIoModule.at(j)->nodeId
                    && E_DEVICE_PROTOCOL_ETHERCAT==logicRobotIo->allIoMap.at(i).deviceProtocol)
            {
                errType=1;
                find=true;
                switch( logicRobotIo->allIoMap.at(i).valueType )
                {
                case    E_IoValueType_NULL:
                    errType=0;
                    break;
                case    E_IoValueType_DI:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< ethercatIoModule.at(j)->diCount )
                    {
                        errType=0;
                    }
                    break;
                case       E_IoValueType_DO:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< ethercatIoModule.at(j)->doCount )
                    {
                        errType=0;
                    }
                    break;
                case      E_IoValueType_AI:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< ethercatIoModule.at(j)->aiCount )
                    {
                        errType=0;
                    }
                    break;
                case     E_IoValueType_AO:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< ethercatIoModule.at(j)->aoCount )
                    {
                        errType=0;
                    }
                    break;
                case         E_IoValueType_SI:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< ethercatIoModule.at(j)->diCount )
                    {
                        errType=0;
                    }
                    break;
                case      E_IoValueType_SO:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< ethercatIoModule.at(j)->doCount )
                    {
                        errType=0;
                    }
                    break;
                }

                if(  errType  )
                {
                    // logicRobotIo->allIoMap.at(i).deviceValueIndex +1 （显示：0开始改为1开始）
//                    addMsg(ENUM_MSG_WARNING, "RobotBase", "RobotIo", 5601,  robotInfo.robotId ,
//                           logicRobotIo->allIoMap.at(i).nodeId,  logicRobotIo->allIoMap.at(i).deviceValueIndex +1  );
//                    return false;

                    //                    qDebug()<<     "i="<<            i << "j="<<j;//ID,
                    //                    qDebug()<<     "ID="<<             IO->allIoMap.at(i).nodeId;//ID,
                    //                    qDebug()<<       "type="<<            IO->allIoMap.at(i). valueType;//状态 ,  si ,so ,di,do ,ai ,ao
                    //                    qDebug()<<      "物理IO索引="<<                    IO->allIoMap.at(i). deviceValueIndex;//物理IO索引
                    //                    qDebug()<<     "逻辑IO索引="<<                    IO->allIoMap.at(i). logicValueIndex;//逻辑IO索引
                    //                    qDebug()<<      "设备类型="<<                   IO->allIoMap.at(i). devType;//设备类型
                    //                    qDebug()<<       "描述="<<                  IO->allIoMap.at(i). note;//描述
                    //                    //  break;
                }
            }
        }
        //CAN
        for( int j=0;j<canopenIoModule.size();j++ )
        {
            if( logicRobotIo->allIoMap.at(i).nodeId==canopenIoModule.at(j)->alias
                    && E_DEVICE_PROTOCOL_CANOPEN==logicRobotIo->allIoMap.at(i).deviceProtocol)
            {
                errType=1;
                find=true;
                switch( logicRobotIo->allIoMap.at(i).valueType )
                {
                case    E_IoValueType_NULL:
                    errType=0;
                    break;
                case    E_IoValueType_DI:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< canopenIoModule.at(j)->diCount )
                    {
                        errType=0;
                    }
                    break;
                case       E_IoValueType_DO:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< canopenIoModule.at(j)->doCount )
                    {
                        errType=0;
                    }
                    break;
                case      E_IoValueType_AI:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< canopenIoModule.at(j)->aiCount )
                    {
                        errType=0;
                    }
                    break;
                case     E_IoValueType_AO:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< canopenIoModule.at(j)->aoCount )
                    {
                        errType=0;
                    }
                    break;
                case         E_IoValueType_SI:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< canopenIoModule.at(j)->diCount )
                    {
                        errType=0;
                    }
                    break;
                case      E_IoValueType_SO:
                    if(  logicRobotIo->allIoMap.at(i).deviceValueIndex< canopenIoModule.at(j)->doCount )
                    {
                        errType=0;
                    }
                    break;
                }

                if(  errType  )
                {
                    // logicRobotIo->allIoMap.at(i).deviceValueIndex +1 （显示：0开始改为1开始）
//                    addMsg(ENUM_MSG_WARNING, "RobotBase", "RobotIo", 5601,  robotInfo.robotId ,
//                           logicRobotIo->allIoMap.at(i).nodeId,  logicRobotIo->allIoMap.at(i).deviceValueIndex +1  );
//                    return false;

                    //                    qDebug()<<     "i="<<            i << "j="<<j;//ID,
                    //                    qDebug()<<     "ID="<<             IO->allIoMap.at(i).nodeId;//ID,
                    //                    qDebug()<<       "type="<<            IO->allIoMap.at(i). valueType;//状态 ,  si ,so ,di,do ,ai ,ao
                    //                    qDebug()<<      "物理IO索引="<<                    IO->allIoMap.at(i). deviceValueIndex;//物理IO索引
                    //                    qDebug()<<     "逻辑IO索引="<<                    IO->allIoMap.at(i). logicValueIndex;//逻辑IO索引
                    //                    qDebug()<<      "设备类型="<<                   IO->allIoMap.at(i). devType;//设备类型
                    //                    qDebug()<<       "描述="<<                  IO->allIoMap.at(i). note;//描述
                    //                    //  break;
                }
            }
        }
        //MODBUS?

//        if( !find )//motor时错报
//        {
//            printf("io map error! not found \n");
//            addMsg( ENUM_MSG_WARNING, "Robotbase", "RobotIo", 5602,  robotInfo.robotId , logicRobotIo->allIoMap.at(i).nodeId );
//            return false;
//        }
    }
    for( int j=0;j<ethercatIoModule.size();j++ )
    {
        ecatIoDeviceDataMap.insert( ethercatIoModule.at(j)->nodeId , ethercatIoModule.at(j) );
    }
    for( int j=0;j<canopenIoModule.size();j++ )
    {
        canioDeviceDataMap.insert( canopenIoModule.at(j)->alias , canopenIoModule.at(j) );
    }
    return true;
}



void RobotBase::test_demo()
{

    if( test_target_increment_set==0 )
        return;

    if(test_run_set){

        if(test_acceleration==0)
            test_dir_tmp=test_dir_set;

        if( abs(test_acceleration)<test_acceleration_set )
        {
            if(test_dir_tmp!=test_dir_set)
            {
                if(test_acceleration>0)
                    test_acceleration--;
                else if(test_acceleration<0)
                    test_acceleration++;
            }else{
                if(test_dir_set)
                    test_acceleration++;
                else
                    test_acceleration--;
            }
            //    printf("run==0==  %d, %d\n",  test_acceleration, test_target_increment_tmp);
        }else{
            if(test_dir_tmp!=test_dir_set)
            {
                if(test_acceleration>0)
                    test_acceleration--;
                else if(test_acceleration<0)
                    test_acceleration++;
            }
        }

        if( abs(test_target_increment_tmp)<=test_target_increment_set )
        {
            int add = test_target_increment_set/test_acceleration_set;
            if(add>0)
                test_target_increment_tmp = add*test_acceleration;
            else
                test_target_increment_tmp = test_acceleration;
        }
        if( abs(test_target_increment_tmp)>test_target_increment_set )
        {
            if(test_dir_set)
                test_target_increment_tmp = test_target_increment_set;
            else
                test_target_increment_tmp = -test_target_increment_set;
        }
    }else{

        //test_dir_tmp=test_dir_set;
        if( abs(test_acceleration)>0 ){
            if(test_acceleration>0)
                test_acceleration--;
            else if(test_acceleration<0)
                test_acceleration++;
            //  printf("run==1==  %d, %d\n",  test_acceleration, test_target_increment_tmp);
        }

        if( abs(test_target_increment_tmp)>=0 ){
            int add = test_target_increment_set/test_acceleration_set;
            if(add>0){
                test_target_increment_tmp = add*test_acceleration;
            }else{
                test_target_increment_tmp = test_acceleration;
            }

            if( test_acceleration==0 && test_target_increment_tmp!=0 )
            {
                test_target_increment_tmp = 0;
                get_data_flag=false;
            }
        }
    }

    if( test_target_increment_tmp!=0 ){
        if( isFaultOccurred||isQuickStopCtl )
            return;

        if( test_axis_set<0 ){
            for(int i=0;i<logicAxis.size();i++)
            {
                logicAxis.at(i)->targetPosition += test_target_increment_tmp ;
            }
        }else{
            if( test_axis_set<logicAxis.size()){
                logicAxis.at(test_axis_set)->targetPosition += test_target_increment_tmp ;
            }
        }

        if( test_dir_ms>0 ){
            if( test_count<test_dir_ms){
                test_count++;
            }else{
                test_count=0;
                if( test_dir_set )
                    test_dir_set=0;
                else
                    test_dir_set=1;
            }
        }
        get_data_flag=true;
    }else{
        test_count=0;
    }

    if(test_target_increment_tmp!=0
            && abs(test_target_increment_tmp)!=test_target_increment_set )
    {
        //printf("run==%d, %d, %d\n",test_target_increment_tmp, test_acceleration, test_target_increment_set);
    }

}

bool RobotBase::isAxisNumberCorrect()
{

    if(logicAxis.size()!=ethercatAxis.size()+modbusRtuAxis.size()+modbusTcpAxis.size()+canopenAxis.size())
    {
        errorCount++;
        if(0==errorCount%(7000000/cycle_us))//wait 7s
        {
               qDebug()<<"errorCount="<<errorCount;
               qDebug()<<"logicAxis.size()!=axis.size()+modbusRtuAxis.size()+modbusTcpAxis.size()";
               addMsg(ENUM_MSG_ERROR, "RobotBase", "RobotBase", 5605,robotInfo.robotId,
                    logicAxis.size(),ethercatAxis.size(),modbusRtuAxis.size()+modbusTcpAxis.size()
                      ,canopenAxis.size());
        }
        return false;
    }

    return true;
}
